#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/string.hpp>
#include <string>
#include <filesystem>
#include <iostream>
#include <opencv2/opencv.hpp>
#include <opencv2/core.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/objdetect.hpp>
#include <vector>
#include <queue>
#include <deque>
#include <limits>
#include <chrono> // 时间库头文件
#include <thread> // 用于模拟耗时操作

using namespace cv;

#define DR (0)
#define DL (1)
#define DT (2)
#define DB (3)
#define RT (4)
#define LT (5)
#define RB (6)
#define LB (7)

class Planning : public rclcpp::Node
{
public:
    Planning(std::string name) : Node(name)
    {
        rclcpp::QoS qos(rclcpp::KeepLast{7});
        timer_ = this->create_wall_timer(std::chrono::milliseconds(200), std::bind(&Planning::timer_callback, this));
        // std::thread t_djikstra_proc(std::bind(&Planning::djikstra, this));
        // t_djikstra_proc.detach();

        // 定义图像的宽度和高度
        int width = 640;
        int height = 640;
        int grid_resolution = 10; // 10 pix

        cv::Point start(3, 30);
        cv::Point end(50, 30);
        cv::Rect roi_obj(100, 100, 30, 300);
        cv::Rect roi_obj2(200, 200, 30, 300);

        cv::Mat bgrImage(height, width, CV_8UC3, cv::Scalar(0, 0, 0));
        bgrImage(roi_obj) = cv::Vec3b(255, 255, 255);
        bgrImage(roi_obj2) = cv::Vec3b(255, 255, 255);

        cv::Mat grayImage;
        cv::cvtColor(bgrImage, grayImage, cv::COLOR_BGR2GRAY);
        std::vector<std::vector<int>> grid_map;
        // 遍历灰度图 init grid map
        for (int row = 0; row < grayImage.rows; row += grid_resolution)
        {
            std::vector<int> cols_v;
            for (int col = 0; col < grayImage.cols; col += grid_resolution)
            {
                uint8_t val = grayImage.at<uint8_t>(row, col);
                cols_v.push_back(val);
            }
            grid_map.push_back(cols_v);
        }

        cv::Rect roi_start(start.x * grid_resolution, start.y * grid_resolution, 10, 10);
        bgrImage(roi_start) = cv::Vec3b(255, 255, 0);
        cv::Rect roi_end(end.x * grid_resolution, end.y * grid_resolution, 10, 10);
        bgrImage(roi_end) = cv::Vec3b(0, 255, 255);
        cv::imshow("planning", bgrImage);
        cv::waitKey(100);

        // std::vector<cv::Point> path = djikstra(grid_map, start, end, bgrImage);
        std::vector<cv::Point> path = jps(grid_map, start, end, bgrImage);
        std::cout << "search over path size: " << path.size() << std::endl;
        cv::Scalar color = cv::Scalar(255, 0, 0); // 红颜色
        int thickness = 2;                        // 线条厚度
        // 遍历点集合，除了最后一个点外，每个点与下一个点之间画线
        if (path.size() > 0)
        {
            for (size_t i = 0; i < path.size() - 1; ++i)
            {
                cv::line(bgrImage, path[i] * grid_resolution, path[i + 1] * grid_resolution, color, thickness);
            }
        }

        cv::imshow("planning", bgrImage);
        cv::waitKey(20000);
    }

private:
    rclcpp::TimerBase::SharedPtr timer_;

    const float INF = std::numeric_limits<float>::max(); // 无穷大

    void timer_callback()
    {
    }

    // 定义优先级队列的比较函数struct compareDistance
    typedef struct CompareDistance
    {
        bool operator()(const std::pair<float, cv::Point> &a, const std::pair<float, cv::Point> &b)
        {
            return a.first > b.first;
        }
    } CompareDistance;

    // 判断点是否在地图内
    bool is_valid_point(const int x, const int y, const int rows, const int cols)
    {
        bool ret = false;

        if (x >= 0 && x < cols && y >= 0 && y < rows)
        {
            ret = true;
        }

        return ret;
    }

    float heuristic(const cv::Point cur, const cv::Point target)
    {
        return std::sqrt((target.x - cur.x) * (target.x - cur.x) + (target.y - cur.y) * (target.y - cur.y));
    }

    std::vector<cv::Point> djikstra(const std::vector<std::vector<int>> &grid, const cv::Point start, const cv::Point end, cv::Mat &image)
    {
        std::vector<cv::Point> path;
        int rows = grid.size();
        int cols = grid[0].size();
        std::vector<std::vector<float>> distance(rows, std::vector<float>(cols, INF));
        std::vector<std::vector<cv::Point>> parents(rows, std::vector<cv::Point>(cols, {-1, -1}));
        std::priority_queue<std::pair<float, cv::Point>, std::vector<std::pair<float, cv::Point>>, CompareDistance> pq;
        distance[start.y][start.x] = 0;
        pq.push({0, start});

        while (!pq.empty())
        {
            cv::Point cur = pq.top().second;
            float cur_dis = pq.top().first;
            pq.pop();

            if (cur_dis > distance[cur.y][cur.x])
            {
                std::cout << "not best dis continue" << std::endl;
                continue;
            }

            if (cur.x == end.x && cur.y == end.y)
            {
                std::cout << "found end  " << std::endl;
                cv::Point current = end;
                while (-1 != current.x && -1 != current.y)
                {
                    path.push_back(current);
                    current = parents[current.y][current.x];
                }
                std::reverse(path.begin(), path.end());

                std::cout << "found end return path size: " << path.size() << std::endl;
                return path;
            }

            int dx[8] = {1, -1, 0, 0, 1, -1, 1, -1};
            int dy[8] = {0, 0, 1, -1, 1, 1, -1, -1};

            for (int i = 0; i < 8; ++i)
            {
                int nb_x = (cur.x + dx[i]);
                int nb_y = (cur.y + dy[i]);

                if (is_valid_point(nb_x, nb_y, rows, cols) && 0 == grid[nb_y][nb_x])
                {
                    float nb_dis = distance[cur.y][cur.x] + std::sqrt(fabs(dx[i] * dx[i] + dy[i] * dy[i]));
                    if (nb_dis < distance[nb_y][nb_x])
                    {
                        parents[nb_y][nb_x] = cur;
                        distance[nb_y][nb_x] = nb_dis;
                        pq.push({nb_dis, {nb_x, nb_y}});

                        if (!(nb_x == end.x && nb_y == end.y))
                        {
                            cv::Rect roi(nb_x * 10, nb_y * 10, 10, 10);
                            image(roi) = cv::Vec3b(0, 255, 0);
                            cv::imshow("planning", image);
                            cv::waitKey(1);
                        }
                    }
                }
            }
        }

        return path;
    }

    std::vector<cv::Point> astar(const std::vector<std::vector<int>> &grid, const cv::Point start, const cv::Point end, cv::Mat &image)
    {
        std::vector<cv::Point> path;
        int rows = grid.size();
        int cols = grid[0].size();
        std::vector<std::vector<float>> distance(rows, std::vector<float>(cols, INF));
        std::vector<std::vector<cv::Point>> parents(rows, std::vector<cv::Point>(cols, {-1, -1}));
        std::priority_queue<std::pair<float, cv::Point>, std::vector<std::pair<float, cv::Point>>, CompareDistance> pq;
        distance[start.y][start.x] = 0;
        pq.push({heuristic(start, end), start});

        while (!pq.empty())
        {
            cv::Point cur = pq.top().second;
            float cur_dis = pq.top().first;
            pq.pop();

            if (cur_dis > distance[cur.y][cur.x] + heuristic(cur, end))
            {
                std::cout << "not best dis continue" << std::endl;
                continue;
            }

            if (cur.x == end.x && cur.y == end.y)
            {
                std::cout << "found end  " << std::endl;

                cv::Point current = end;
                while (-1 != current.x && -1 != current.y)
                {
                    path.push_back(current);
                    current = parents[current.y][current.x];
                }
                std::reverse(path.begin(), path.end());

                std::cout << "found end return path size: " << path.size() << std::endl;
                return path;
            }

            int dx[8] = {1, -1, 0, 0, 1, -1, 1, -1};
            int dy[8] = {0, 0, 1, -1, 1, 1, -1, -1};

            for (int i = 0; i < 8; ++i)
            {
                int nb_x = (cur.x + dx[i]);
                int nb_y = (cur.y + dy[i]);

                if (is_valid_point(nb_x, nb_y, rows, cols) && 0 == grid[nb_y][nb_x])
                {
                    float nb_dis = distance[cur.y][cur.x] + std::sqrt(fabs(dx[i] * dx[i] + dy[i] * dy[i]));
                    if (nb_dis < distance[nb_y][nb_x])
                    {
                        parents[nb_y][nb_x] = cur;
                        distance[nb_y][nb_x] = nb_dis;

                        float nb_prio_dis = nb_dis + heuristic({nb_x, nb_y}, end);
                        pq.push({nb_prio_dis, {nb_x, nb_y}});

                        if (!(nb_x == end.x && nb_y == end.y))
                        {
                            cv::Rect roi(nb_x * 10, nb_y * 10, 10, 10);
                            image(roi) = cv::Vec3b(0, 255, 0);
                            cv::imshow("planning", image);
                            cv::waitKey(1);
                        }
                    }
                }
            }
        }

        return path;
    }

    //寻找跳点
    bool has_force_neibor_node(const std::vector<std::vector<int>> &grid, std::vector<std::vector<float>> &close_list, int nb_x, int nb_y, int dir, const cv::Point start, const cv::Point end, cv::Point &jump_point, cv::Mat &image)
    {
        bool ret = false;
        int rows = grid.size();
        int cols = grid[0].size();
        static cv::Vec3b color = cv::Vec3b(0, 255, 255);
        static int8_t color_idx = 1;

        if (0 == color_idx)
        {
            color = cv::Vec3b(255, 0, 0);
        }
        else if (1 == color_idx)
        {
            color = cv::Vec3b(0, 255, 0);
        }
        else if (2 == color_idx)
        {
            color = cv::Vec3b(0, 0, 255);
            color_idx = -1;
        }
        // color_idx++;

        int tmp_nb_x = nb_x;
        int tmp_nb_y = nb_y;

        while (1)
        {
            //向右遍历节点
            if (RT != dir && RB != dir && DR != dir)
            {
                break;
            }

            //如果遇到地图边界或者遇到障碍物则直接停止这个方向的搜索
            if (tmp_nb_x >= rows || grid[tmp_nb_y][tmp_nb_x] > 0)
            {
                break;
            }

            //将该点标记为所搜过的点
            close_list[tmp_nb_y][tmp_nb_x] = 1;

            //边界值判断
            if (tmp_nb_y - 1 >= 0 && tmp_nb_y + 1 < rows)
            {
                //如果上边有强制邻居则说明这个节点是跳点，返回改跳点
                if (grid[tmp_nb_y + 1][tmp_nb_x] > 0 && 0 == grid[tmp_nb_y + 1][tmp_nb_x + 1])
                {
                    // return true;
                    ret = true;
                    jump_point = {tmp_nb_x, tmp_nb_y};
                    break;
                }

                //如果下边有强制邻居则说明这个节点是跳点，返回改跳点
                if (grid[tmp_nb_y - 1][tmp_nb_x] > 0 && 0 == grid[tmp_nb_y - 1][tmp_nb_x + 1])
                {
                    // return true;
                    ret = true;
                    jump_point = {tmp_nb_x, tmp_nb_y};
                    break;
                }
            }

            //终点也是跳点
            if (end.x == tmp_nb_x && end.y == tmp_nb_y)
            {
                // return true;
                ret = true;
                jump_point = {tmp_nb_x, tmp_nb_y};
                break;
            }

            //画出探索过的点
            if (!(start.x == tmp_nb_x && start.y == tmp_nb_y))
            {
                cv::Rect roi(tmp_nb_x * 10, tmp_nb_y * 10, 10, 10);
                image(roi) = color;
                cv::imshow("planning", image);
            }
            tmp_nb_x++;
        }

        tmp_nb_x = nb_x;
        tmp_nb_y = nb_y;
        while (1)
        {
            //向左遍历节点
            if (LT != dir && LB != dir && DL != dir)
            {
                break;
            }

            if (tmp_nb_x < 0 || grid[tmp_nb_y][tmp_nb_x] > 0)
            {
                break;
            }

            close_list[tmp_nb_y][tmp_nb_x] = 1;

            if (tmp_nb_y - 1 >= 0 && tmp_nb_y + 1 < rows)
            {
                if (grid[tmp_nb_y + 1][tmp_nb_x] > 0 && 0 == grid[tmp_nb_y + 1][tmp_nb_x - 1])
                {
                    // return true;
                    ret = true;
                    jump_point = {tmp_nb_x, tmp_nb_y};
                    break;
                }

                if (grid[tmp_nb_y - 1][tmp_nb_x] > 0 && 0 == grid[tmp_nb_y - 1][tmp_nb_x - 1])
                {
                    // return true;
                    ret = true;
                    jump_point = {tmp_nb_x, tmp_nb_y};
                    break;
                }
            }

            if (end.x == tmp_nb_x && end.y == tmp_nb_y)
            {
                // return true;
                ret = true;
                jump_point = {tmp_nb_x, tmp_nb_y};
                break;
            }

            if (!(start.x == tmp_nb_x && start.y == tmp_nb_y))
            {
                cv::Rect roi(tmp_nb_x * 10, tmp_nb_y * 10, 10, 10);
                image(roi) = color;
                cv::imshow("planning", image);
            }

            tmp_nb_x--;
        }

        tmp_nb_x = nb_x;
        tmp_nb_y = nb_y;
        while (1)
        {
            //向上遍历节点
            if (LT != dir && RT != dir && DT != dir)
            {
                break;
            }

            if (tmp_nb_y >= cols || grid[tmp_nb_y][tmp_nb_x] > 0)
            {
                break;
            }

            close_list[tmp_nb_y][tmp_nb_x] = 1;

            if (tmp_nb_x - 1 >= 0 && tmp_nb_x + 1 < cols)
            {
                if (grid[tmp_nb_y][tmp_nb_x + 1] > 0 && 0 == grid[tmp_nb_y + 1][tmp_nb_x + 1])
                {
                    // return true;
                    ret = true;
                    jump_point = {tmp_nb_x, tmp_nb_y};
                    break;
                }

                if (grid[tmp_nb_y][tmp_nb_x - 1] > 0 && 0 == grid[tmp_nb_y + 1][tmp_nb_x - 1])
                {
                    // return true;
                    ret = true;
                    jump_point = {tmp_nb_x, tmp_nb_y};
                    break;
                }
            }

            if (end.x == tmp_nb_x && end.y == tmp_nb_y)
            {
                // return true;
                ret = true;
                jump_point = {tmp_nb_x, tmp_nb_y};
                break;
            }

            if (!(start.x == tmp_nb_x && start.y == tmp_nb_y))
            {
                cv::Rect roi(tmp_nb_x * 10, tmp_nb_y * 10, 10, 10);
                image(roi) = color;
                cv::imshow("planning", image);
            }

            tmp_nb_y++;
        }

        tmp_nb_x = nb_x;
        tmp_nb_y = nb_y;
        while (1)
        {
            //向下遍历节点
            if (LB != dir && RB != dir && DB != dir)
            {
                break;
            }

            if (tmp_nb_y < 0 || grid[tmp_nb_y][tmp_nb_x] > 0)
            {
                break;
            }

            close_list[tmp_nb_y][tmp_nb_x] = 1;

            if (tmp_nb_x - 1 >= 0 && tmp_nb_x + 1 < cols)
            {
                if (grid[tmp_nb_y][tmp_nb_x + 1] > 0 && 0 == grid[tmp_nb_y - 1][tmp_nb_x + 1])
                {
                    // return true;
                    ret = true;
                    jump_point = {tmp_nb_x, tmp_nb_y};
                    break;
                }

                if (grid[tmp_nb_y][tmp_nb_x - 1] > 0 && 0 == grid[tmp_nb_y - 1][tmp_nb_x - 1])
                {
                    // return true;
                    ret = true;
                    jump_point = {tmp_nb_x, tmp_nb_y};
                    break;
                }
            }

            if (end.x == tmp_nb_x && end.y == tmp_nb_y)
            {
                // return true;
                ret = true;
                jump_point = {tmp_nb_x, tmp_nb_y};
                break;
            }

            if (!(start.x == tmp_nb_x && start.y == tmp_nb_y))
            {
                cv::Rect roi(tmp_nb_x * 10, tmp_nb_y * 10, 10, 10);
                image(roi) = color;
                cv::imshow("planning", image);
            }

            tmp_nb_y--;
        }

        return ret;
    }

    std::vector<cv::Point> jps(const std::vector<std::vector<int>> &grid, const cv::Point start, const cv::Point end, cv::Mat &image)
    {
        std::vector<cv::Point> path;
        int rows = grid.size();
        int cols = grid[0].size();
        std::vector<std::vector<float>> close_list(rows, std::vector<float>(cols, 0));
        std::vector<std::vector<float>> distance(rows, std::vector<float>(cols, INF));
        std::vector<std::vector<cv::Point>> parents(rows, std::vector<cv::Point>(cols, {-1, -1}));
        std::priority_queue<std::pair<float, cv::Point>, std::vector<std::pair<float, cv::Point>>, CompareDistance> pq;
        distance[start.y][start.x] = 0;
        pq.push({heuristic(start, end), start});

        while (!pq.empty())
        {
            cv::Point cur = pq.top().second;
            float cur_dis = pq.top().first;
            pq.pop();
            std::cout << "pq push size: " << pq.size() << std::endl;

            //如果当前点之前有访问过并且他的cost不是最小的则直接跳过
            if (cur_dis > distance[cur.y][cur.x] + heuristic(cur, end))
            {
                std::cout << "not best dis continue" << std::endl;
                continue;
            }

            //如果找到终点则回溯最优路径后返回路径
            if (cur.x == end.x && cur.y == end.y)
            {
                std::cout << "found end  " << std::endl;

                cv::Point current = end;
                while (-1 != current.x && -1 != current.y)
                {
                    path.push_back(current);
                    current = parents[current.y][current.x];
                }
                std::reverse(path.begin(), path.end());

                std::cout << "found end return path size: " << path.size() << std::endl;
                return path;
            }

            //8联通图
            int dx[8] = {1, -1, 0, 0, 1, -1, 1, -1};
            int dy[8] = {0, 0, 1, -1, 1, 1, -1, -1};

            //遍历周围一圈8个节点
            for (int i = 0; i < 8; ++i)
            {
                int nb_x = (cur.x + dx[i]);
                int nb_y = (cur.y + dy[i]);

                if (is_valid_point(nb_x, nb_y, rows, cols) && 0 == grid[nb_y][nb_x])
                {
                    //jps是astar的特殊优化版本，代码方面区别在于Astart是将当前节点的所有邻居节点
                    //都遍历一遍计算出cost然后入列，由于是优先级队列所以每次出列都是距离终点最近的节点
                    //所以都是优先搜索距离终点最近的节点方向
                    //jps前面的步骤和Astart一样区别则是在8连通图的8个方向上不停的寻找障碍物（已经访问过的点不再访问），
                    //然后找到强制邻居确定跳点，然后只需要将跳点和对角线的点入列即可
                    //
                    if (0 == close_list[nb_y][nb_x]) //已经访问过的点不再访问
                    {
                        int dir = i;
                        std::cout << "===================================================== [" << i << "]cur nb: " << nb_x << "," << nb_y << std::endl;
                        auto start_time = std::chrono::high_resolution_clock::now();
                        cv::Point jump_point;
                        //寻找强制邻居和跳点
                        bool ret_nb = has_force_neibor_node(grid, close_list, nb_x, nb_y, dir, start, end, jump_point, image);
                        auto end_time = std::chrono::high_resolution_clock::now();
                        auto duration = std::chrono::duration_cast<std::chrono::milliseconds>(end_time - start_time);
                        // std::cout << "Manual timing: " << duration.count() << " ms\n";

                        std::cout << "ret_nb: " << ret_nb << std::endl;
                        //如果是未访问过的对角线邻居或者是跳点则需要入列openlist
                        if (ret_nb || (i >= RT))
                        {
                            float nb_dis = distance[cur.y][cur.x] + std::sqrt(fabs(dx[i] * dx[i] + dy[i] * dy[i]));
                            //如果当前邻居点通过当前父节点的cost小于经过之前父节点的cost才需要加入到openlist队列，
                            //这样才能确保路径最优
                            if (nb_dis < distance[nb_y][nb_x])
                            {
                                
                                parents[nb_y][nb_x] = cur;
                                distance[nb_y][nb_x] = nb_dis;
                                float nb_prio_dis = nb_dis + heuristic({nb_x, nb_y}, end);
                                std::cout << "pq push: " << nb_x << "," << nb_y << ", cur:" << cur << std::endl;
                                pq.push({nb_prio_dis, {nb_x, nb_y}});//将邻居入列
                                close_list[nb_y][nb_x] = 1;

                                //如果这次有找到跳点,并且邻居节点本身不是跳点,则将跳点也入openlist列
                                if (ret_nb && !(nb_y == jump_point.y && nb_x == jump_point.x))
                                {
                                    parents[jump_point.y][jump_point.x] = {nb_x, nb_y};
                                    distance[jump_point.y][jump_point.x] = nb_dis + std::sqrt((jump_point.y - nb_y) * (jump_point.y - nb_y) + (jump_point.x - nb_x) * (jump_point.x - nb_x));
                                    float nb_prio_dis = distance[jump_point.y][jump_point.x] + heuristic({jump_point.x, jump_point.y}, end);
                                    std::cout << "pq jmp pt push: " << jump_point.x << "," << jump_point.y << std::endl;
                                    pq.push({nb_prio_dis, {jump_point.x, jump_point.y}});
                                }

                                //画出跳点和强制邻居节点
                                if (!(nb_x == end.x && nb_y == end.y) && ret_nb)
                                {
                                    cv::Rect roi(jump_point.x * 10, jump_point.y * 10, 10, 10);
                                    image(roi) = cv::Vec3b(0, 255, 255);
                                    cv::imshow("planning", image);

                                    cv::Rect roi2(nb_x * 10, nb_y * 10, 10, 10);
                                    image(roi2) = cv::Vec3b(0, 255, 255);
                                    cv::imshow("planning", image);
                                    // cv::waitKey(1);
                                }
                            }
                        }

                        //加点延时方便观察过程
                        cv::waitKey(10);
                    }
                }
            }
        }

        return path;
    }
};

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<Planning>("Planning");
    rclcpp::spin(node);
    rclcpp::shutdown();

    return EXIT_SUCCESS;
}